#include "v4l2camera.h"
#include <QDebug>
v4l2camera::v4l2camera(QObject *parent)
    : QThread{parent}
{
    v4l2_init();
    stopflag=0;
}

v4l2camera::~v4l2camera()
{
    int ret;
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ret = ioctl(camerafd,VIDIOC_STREAMOFF,&type);
    if(ret < 0){
        qDebug()<<"Fail to OFF";
    }
    for(uint8_t i=0;i<4;i++)
    {
        munmap(buffer[i].start,buffer[i].len);
        buffer[i].start=NULL;
    }
    free(currentstart.start);
    close(camerafd);
    stopflag=0;
    qDebug()<<"close Thread";

}
int v4l2camera:: convert_yuv_to_rgb_pixel(int y, int u, int v)
{
    unsigned int pixel32 = 0;
    unsigned char *pixel = (unsigned char *)&pixel32;
    int r, g, b;
    r = y + (1.370705 * (v-128));
    g = y - (0.698001 * (v-128)) - (0.337633 * (u-128));
    b = y + (1.732446 * (u-128));
    if(r > 255) r = 255;
    if(g > 255) g = 255;
    if(b > 255) b = 255;
    if(r < 0) r = 0;
    if(g < 0) g = 0;
    if(b < 0) b = 0;
    pixel[0] = r * 220 / 256;
    pixel[1] = g * 220 / 256;
    pixel[2] = b * 220 / 256;
    return pixel32;
}

int v4l2camera:: convert_yuv_to_rgb_buffer(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height)
{
    unsigned int in, out = 0;
    unsigned int pixel_16=0;
    unsigned char pixel_24[3];
    unsigned int pixel32=0;
    int y0, u, y1, v;
    for(in = 0; in < width * height * 2; in += 4) {
        pixel_16 = yuv[in + 3] << 24 | yuv[in + 2] << 16 | yuv[in + 1] <<  8 | yuv[in + 0];
        y0 = (pixel_16 & 0x000000ff);
        u  = (pixel_16 & 0x0000ff00) >>  8;
        y1 = (pixel_16 & 0x00ff0000) >> 16;
        v  = (pixel_16 & 0xff000000) >> 24;

        pixel32 = convert_yuv_to_rgb_pixel(y0, u, v);

        pixel_24[0] = (pixel32 & 0x000000ff);

        pixel_24[1] = (pixel32 & 0x0000ff00) >> 8;
        pixel_24[2] = (pixel32 & 0x00ff0000) >> 16;

        rgb[out++] = pixel_24[0];
        rgb[out++] = pixel_24[1];
        rgb[out++] = pixel_24[2];


        pixel32 = convert_yuv_to_rgb_pixel(y1, u, v);

        pixel_24[0] = (pixel32 & 0x000000ff);
        pixel_24[1] = (pixel32 & 0x0000ff00) >> 8;
        pixel_24[2] = (pixel32 & 0x00ff0000) >> 16;
        rgb[out++] = pixel_24[0];
        rgb[out++] = pixel_24[1];
        rgb[out++] = pixel_24[2];
    }
    return 0;
}

void v4l2camera::run()
{
    int ret=0;


        while(1)
        {
            struct v4l2_buffer buf;
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_MMAP;
            ret= ioctl(camerafd, VIDIOC_DQBUF, &buf);
            if(ret<0)
            {
                qDebug()<<"DQBUF error";
            }
            memcpy(currentstart.start,buffer[buf.index].start,buf.bytesused);
            currentstart.len=buf.bytesused;

            ret= ioctl(camerafd, VIDIOC_QBUF, &buf);
            if(ret<0)
            {
                qDebug()<<"QBUF error";
            }

            convert_yuv_to_rgb_buffer(currentstart.start, rgb_buffer, 640,480);
            QImage image(rgb_buffer, 640,480, QImage::Format_RGB888);
            emit sendimage(image);



            if (isInterruptionRequested())
            {
                break;
            }
        }
        emit thread_complete();
}

int v4l2camera::v4l2_init()
{
    int ret=0;
    camerafd = open(video_device.toStdString().c_str(),O_RDWR);
    if(camerafd < 0){
        qDebug() << "fail to open video_device\n";
        return -1;
    }

    //获取功能参数
    struct v4l2_capability cap;
    ret = ioctl(camerafd, VIDIOC_QUERYCAP, &cap);
    if(ret < 0)
        qDebug()<<"cap error";
    if(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
        qDebug()<<"is a camera";

   struct v4l2_fmtdesc fmt;
    // memset(&fmt, 0, sizeof(fmt));
    // fmt.index=0;
    // fmt.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
   fmt.index = 0;
   fmt.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    for(fmt.index=0;fmt.index<4;fmt.index++)
    {
        ret = ioctl(camerafd, VIDIOC_ENUM_FMT, &fmt);
        if(ret==-1)
            perror("error");
        qDebug()<<fmt.description;
    }

    //设置参数
    struct v4l2_format format={0};
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;          /* 视频采集 */
    format.fmt.pix.width = 640  ;                 /* 宽 */
    format.fmt.pix.height = 480;                /* 高 */
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    //format.fmt.pix.field=V4L2_FIELD_NONE;

    ret=ioctl(camerafd, VIDIOC_S_FMT, &format);
        if(ret ==-1){
            perror("Error");
            close(camerafd);
            return -1;
        }
    // while(ioctl(camerafd, VIDIOC_S_FMT, &format)==-1)
    // {
    //     // if(ret ==-1){
    //         perror("Error");
    //         // close(camerafd);
    //         // return -1;
    // }


    //申请缓存空间
    memset(&requestbuffers, 0, sizeof(struct v4l2_requestbuffers));
    requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    requestbuffers.count = 4;                       //申请缓存个数
    requestbuffers.memory = V4L2_MEMORY_MMAP;
    ret = ioctl(camerafd, VIDIOC_REQBUFS, &requestbuffers);
    if(ret < 0){
        qDebug()<<"mem request error";
        close(camerafd);
        return -1;
    }

    //映射，入队
    for(uint8_t i=0;i<4;i++)
    {
        struct v4l2_buffer buf={};
        buf.index=i;
        buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory=V4L2_MEMORY_MMAP;

        ret = ioctl(camerafd, VIDIOC_QUERYBUF, &buf);
        if(ret ==-1){
            qDebug()<<"memset fail";
            close(camerafd);
            return -1;
        }
        maxlength=buf.length;
        buffer[i].len=buf.length;
        buffer[i].start=(unsigned char*)mmap(NULL,buf.length,PROT_READ | PROT_WRITE,MAP_SHARED,camerafd,buf.m.offset);
        if(buffer[i].start==MAP_FILE)
        {
            qDebug()<<"mmap error";
            close(camerafd);
            return -1;
        }

        ret = ioctl(camerafd,VIDIOC_QBUF,&buf);
        if(ret < 0){
            qDebug()<<"Fail to ioctl:VIDIOC_QBUF";
            close(camerafd);
            return -1;
        }
    }

    currentstart.start=(unsigned char*)malloc(maxlength);
    if(currentstart.start==NULL)
    {
        qDebug()<<"currentstart error";
    }

    //开启摄像头
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ret = ioctl(camerafd,VIDIOC_STREAMON,&type);
    if(ret < 0){
        qDebug()<<"Fail to ioctl:VIDIOC_STREAMON";
        return -1;
    }

    return 0;
}

void v4l2camera::stop_signal()
{
    stopflag=1;
}
